#include <stdio.h>      /* for printf() and fprintf() */
#include <sys/socket.h> /* for socket(), bind(), and connect() */
#include <arpa/inet.h>  /* for sockaddr_in and inet_ntoa() */
#include <stdlib.h>     /* for atoi() and exit() */
#include <string.h>     /* for memset() */
#include <unistd.h>     /* for close() */

#include <roboMoMo.h>

#define CONF_SIZE 4

// The following constants are for referencing devices in config array
// Their value is their index in the array

#define MOTOR0  0
#define MOTOR1  1
#define SERVO0  2
#define BUZZER0 3
#define MAXPENDING 5    /* Maximum outstanding connection requests */
#define RCVBUFSIZE 32   /* Size of receive buffer */


static char const *devices[CONF_SIZE] = {
	"Motor0",
	"Motor1",
	"Servo0",
	"Buzzer0"
};

void DieWithError(char *errorMessage)
{
	perror(errorMessage);
	exit(1);
}

void runCmd(char *cmdBuffer, int device, int arg)
{
	if(cmdBuffer[0] == 'm') {
		setMotorValue(device, arg);
		printf("Setting %s value to %d.\n", devices[device], arg);
	}
	else if(cmdBuffer[0] == 'b') {
		printf("SettingBuzzerValue.\n");
	}
}

void HandleTCPClient(int clntSocket)
{
	char echoBuffer[RCVBUFSIZE];        /* Buffer for echo string */
	int recvMsgSize;                    /* Size of received message */

	char cmdBuffer[RCVBUFSIZE];
	int device = 0;
	int arg = 0;

	/* Receive message from client */
	if ((recvMsgSize = recv(clntSocket, echoBuffer, RCVBUFSIZE, 0)) < 0)
		DieWithError("recv() failed");

	memset(cmdBuffer, 0, RCVBUFSIZE);
	sscanf(echoBuffer, "%s %d %d", cmdBuffer, &device, &arg);

	runCmd(cmdBuffer, device, arg);

	/* Send received string and receive again until end of transmission */
	while (recvMsgSize > 0) {
		/* Echo message back to client */
//		if (send(clntSocket, echoBuffer, recvMsgSize, 0) != recvMsgSize)
//			DieWithError("send() failed");

		/* See if there is more data to receive */
		if ((recvMsgSize = recv(clntSocket, echoBuffer, RCVBUFSIZE, 0)) < 0)
			DieWithError("recv() failed");
	}

	close(clntSocket);    /* Close client socket */
}

int main(int argc, char *argv[])
{
	// The config array is an array of structs and is initialized as such
	// roboMoMo simply makes referencing the device files pretty but the
	// device files used by the drivers must be given in the config

	struct robo_dev config[CONF_SIZE] = {
		{ .value_file = "/sys/devices/msc0/motor0/value", .type = motor_type},
		{ .value_file = "/sys/devices/msc0/motor1/value", .type = motor_type},
		{ .value_file = "/sys/devices/msc0/servo0/value", .type = servo_type},
		{ .value_file = "/sys/devices/msc0/buzzer0/value", .type = buzzer_type}
	};

	// After the config is set using set config simply changing the array
	// will not change the config as the array is duplicated by setConfig

	setConfig(config, CONF_SIZE);

	// When referencing devices with roboMoMo the first argument is always
	// the index of the device in the config array

	int servSock;                    /* Socket descriptor for server */
	int clntSock;                    /* Socket descriptor for client */
	struct sockaddr_in echoServAddr; /* Local address */
	struct sockaddr_in echoClntAddr; /* Client address */
	unsigned short echoServPort;     /* Server port */
	unsigned int clntLen;            /* Length of client address data structure */

	echoServPort = 9999;  /* First arg:  local port */

	/* Create socket for incoming connections */
	if ((servSock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0)
		DieWithError("socket() failed");

	/* Construct local address structure */
	memset(&echoServAddr, 0, sizeof(echoServAddr));   /* Zero out structure */
	echoServAddr.sin_family = AF_INET;                /* Internet address family */
	echoServAddr.sin_addr.s_addr = htonl(INADDR_ANY); /* Any incoming interface */
	echoServAddr.sin_port = htons(echoServPort);      /* Local port */

	/* Bind to the local address */
	if (bind(servSock, (struct sockaddr *) &echoServAddr, 
		sizeof(echoServAddr)) < 0)
	DieWithError("bind() failed");

	/* Mark the socket so it will listen for incoming connections */
	if (listen(servSock, MAXPENDING) < 0)
		DieWithError("listen() failed");

	for (;;) {
		/* Set the size of the in-out parameter */
		clntLen = sizeof(echoClntAddr);

		/* Wait for a client to connect */
		if ((clntSock = accept(servSock, (struct sockaddr *) &echoClntAddr, &clntLen)) < 0)
			DieWithError("accept() failed");

		HandleTCPClient(clntSock);
	}
	/* NOT REACHED */
}

/*
	int i;

	setMotorValue(MOTOR1, 90);

	sleep(3);

	setMotorValue(MOTOR0, 90);
	setMotorValue(MOTOR1, -90);

	sleep(1);

	setMotorValue(MOTOR0, 90);
	setMotorValue(MOTOR1, 90);

	sleep(3);

	setMotorValue(MOTOR0, 0);
	setMotorValue(MOTOR1, 0);

	disableServo(SERVO0);

	return 0;
}*/
